/*
superSonic
提供倍加福超声波传感器的驱动功能，主要为获取集群传感器数据，解析数据得到距离数据，单位cm
designed by lunanting

-----------------------
2020-04-10 13:22:20
create file
-----------------------
*/

#include "superSonic.h"
#include "aos/hal/uart.h"
#include "cli_cmd.h"
#include "stdlib.h"
#include "string.h"


static SUPERS_handler supers;

/*
init super sonic sensor
param:
    port:uart port
    speed:uart speed
*/
int xp_supers_init(u8 port,u32 speed){
    uart_dev_t *uart=&supers.uart;
    int sta=0;

    uart->port=port;
    uart->config.baud_rate=speed;
    uart->config.data_width=DATA_WIDTH_8BIT;
    uart->config.parity=NO_PARITY;
    uart->config.stop_bits=STOP_BITS_1;
    uart->config.flow_control=FLOW_CONTROL_DISABLED;
    uart->config.mode=MODE_TX_RX;

    sta=hal_uart_init(uart);
    if(sta!=0){
        supers.init=0;
        println("error:supersonic sensor init fail ! hal_uart_init() return code=%d",sta);
        return -1;
    }
    println("supersonic sensor init success!,by uart port=%d",port);
    sta=aos_sem_new(&supers.sem,1);
    if(sta!=0){
        println("error:xp_supers_init() new sem fail!");
        return -2;
    }
    supers.init=1;
    return 0;
}


/*
send cmd and recive data
param:
    cmd:send data
    data:recive data
return:
    0=success
*/
int xp_supers_ask(u8 *cmd,u8 cmd_len,u8 *data,u8 *data_len,u8 exp_len){
    int sta=0;
    aos_sem_t *sem=&supers.sem;
	u32 len;
    sta=aos_sem_wait(sem,200);
    if(sta!=0){
        // aos_sem_signal(sem);
        return -1;
    }
    sta=hal_uart_send(&supers.uart,cmd,cmd_len,UART_wait);
    if(sta!=0)return -2;
    hal_uart_recv_II(&supers.uart,data,exp_len,&len,UART_wait);
	*(data_len)=len;
    aos_sem_signal(sem);
    return 0;
}


/*
sensor address check
param:
    addr:address
    location:function name
return:
    0=success
    -1=fault input
*/
int xp_supers_addr_check(u8 addr,char *name){
    if(addr<1||addr>7){
        println("error:%s() addr=%d,input error,need rang:1~7!",name,addr);
        return -1;
    }
    return 0;
}

/*
change supersonic sensor address from add1 to addr2
param:
    addr1:origin address
    addr2:object address
return:
    0=success
*/
int xp_supers_set_addr(u8 addr1,u8 addr2){
    u8 cmd[5]={0};
    u8 rec_data[10];
    u8 rec_len;
    int sta=0;
    
    if(xp_supers_addr_check(addr1,"xp_supers_set_addr")!=0){
        return -1;
    }
    if(xp_supers_addr_check(addr2,"xp_supers_set_addr")!=0){
        return -2;
    }
    cmd[0]=0xa0|addr1;
    cmd[1]=0x35;
    cmd[2]=addr2;
    cmd[3]=xp_supers_ask_checksum(cmd,3);

    sta=xp_supers_ask(cmd,4,rec_data,&rec_len,10);
    if(sta!=0)return -3;
    if((rec_data[5]&0x80)&&rec_data[4]==addr2){
        return 0;
    }
    return -4;
}

/*
check sensor online state
param:
    addr:sensor address
return:
    0=offline
    1=online
*/
u8 xp_supers_online_check(u8 addr){
    u8 cmd[5]={0};
    u8 rec_data[10];
    u8 rec_len;
    int sta=0;

    if(xp_supers_addr_check(addr,"xp_supers_online_check")!=0){
        return -1;
    }
    cmd[0]=0xa0|0x08|addr;
    cmd[1]=0x35;
    cmd[2]=0xff;
    cmd[3]=xp_supers_ask_checksum(cmd,3);
    sta=xp_supers_ask(cmd,4,rec_data,&rec_len,6);
    if(sta!=0)return -2;
    if((rec_data[5]&0x80)&&rec_data[4]==addr){
        return 0;
    }
    return -3;
}


/*
read sensor address only one online
return:address,0=get error
*/
u8 xp_supers_get_onlyone_addr(void){
    u8 cmd[5]={0};
    u8 rec_data[10];
    u8 rec_len;
    int sta=0;

    cmd[0]=0xa8;
    cmd[1]=0;
    cmd[2]=0;
    cmd[4]=0x43;
    sta=xp_supers_ask(cmd,4,rec_data,rec_data,6);
    if(sta!=0)return -1;
    if(rec_data[5]&0x80){
        return rec_data[4];
    }
    return 0;
}


/*
factory setting sensor
param:
    addr:set sensor address
return:
    0=success

*/
int xp_supers_factory_set(u8 addr){
    int sta=0;
    u8 cmd[5]={0};
    u8 rec_data[10];
    u8 rec_len;
    
    if(xp_supers_addr_check(addr,"xp_supers_factory_set")!=0){
        return -1;
    }
    cmd[0]=0xa0|addr;
    cmd[1]=0x36;
    cmd[2]=0x55;
    cmd[3]=xp_supers_ask_checksum(cmd,3);
    sta=xp_supers_ask(cmd,4,rec_data,&rec_len,6);
    if(sta!=0)return -2;
    if(rec_data[4]==0xff){
        return 0;
    }
    return -3;
}



/*
get sensor temperature
return:temperature value,255=error
*/
u8 xp_supers_get_temperature(u8 addr){
    int sta=0;
    u8 cmd[5]={0};
    u8 rec_data[10];
    u8 rec_len;

    cmd[0]=0xa0|0x08|addr;
    cmd[1]=0xff;
    cmd[2]=0x01;
    cmd[3]=0x70;

    sta=xp_supers_ask(cmd,4,rec_data,&rec_len,6);
    if(sta!=0)return -1;
    if(rec_data[5]&0x80){
        return rec_data[4];
    }
    return 255;
}


/*
get distance from supersonic sensor
paran:
    addr:sensor address
    type:SUPERS_ABC mesure type
    cycle:cycle
return:distance,0=get error
*/
int xp_supers_get_dis(u8 addr,SUPERS_ABC type,u8 cycle){
    int sta=0;
    u8 cmd[5]={0};
    u8 rec_data[10];
    u8 rec_len;

    if(xp_supers_addr_check(addr,"xp_supers_get_dis")!=0){
        return -1;
    }
    cmd[0]=0xa0|0x08|addr;
    if(type==supers_A){
        cmd[1]=0xfe;
    }
    else if(type==supers_B){
        cmd[1]=0xfd;
    }
    else if(type==supers_C){
        cmd[1]=0xfc;
    }
    else{
        println("error:xp_supers_get_dis() input type=%d error!",type);
        return -2;
    }
    if(cycle==1){
        cmd[2]=0xfe;
    }
    else if(cycle==2){
        cmd[2]=0xfd;
    }
    else{
        println("error:xp_supers_get_dis() input cycle=%d error",cycle);
        return -3;
    }
    cmd[3]=xp_supers_ask_checksum(cmd,3);
    xp_supers_ask(cmd,4,rec_data,&rec_len,10);
	print_buf(rec_data,rec_len,8,16);
    if(rec_data[5]&0x80){
        if(SuperS_type==4000){
            return rec_data[4]*1.6;
        }
        else if(SuperS_type==2500){
            return rec_data[4];
        }
        else{
            return rec_data[4];
        }
    }
    return -1;
}



/*
supersonic sensor ask checksum
param:
    data:data buffer
    len:data len
return:checksum value
*/
u8 xp_supers_ask_checksum(u8 *data,u8 len){
    u8 xor=data[0];
    u8 sum=0;
    u8 d[8]={0};

    for(u8 i=1;i<len;i++){
        xor=xor^data[i];
    }
    xor=xor^0x52;
    d[5]=((xor>>7)^(xor>>5)^(xor>>3)^(xor>>1))&0x01;
    d[4]=((xor>>6)^(xor>>4)^(xor>>2)^xor)&0x01;
    d[3]=((xor>>7)^(xor>>6))&0x01;
    d[2]=((xor>>5)^(xor>>4))&0x01;
    d[1]=((xor>>3)^(xor>>2))&0x01;
    d[0]=((xor>>1)^xor)&0x01;
    d[6]=0x01;
    sum=(d[6]<<6)|(d[5]<<5)|(d[4]<<4)|(d[3]<<3)|(d[2]<<2)|(d[1]<<1)|d[0];
}

/*
supersonic sensor response checksum
param:
    data:check data
return:0=success,-1=fail
*/
int xp_supers_response_checksum(u8 *data){
    u8 bit7=data[1]&0x80;
    u8 xor;
    u8 d[8]={0};
    u8 sum=0;

    xor=0x52^data[0]^bit7;
    d[5]=((xor>>7)^(xor>>5)^(xor>>3)^(xor>>1))&0x01;
    d[4]=((xor>>6)^(xor>>4)^(xor>>2)^xor)&0x01;
    d[3]=((xor>>7)^(xor>>6))&0x01;
    d[2]=((xor>>5)^(xor>>4))&0x01;
    d[1]=((xor>>3)^(xor>>2))&0x01;
    d[0]=((xor>>1)^xor)&0x01;
    d[6]=0x01;
    d[7]=data[1]>>7;
    sum=(d[7]<<7)|(d[6]<<6)|(d[5]<<5)|(d[4]<<4)|(d[3]<<3)|(d[2]<<2)|(d[1]<<1)|d[0];
    if(sum==data[1]){
        return 0;
    }
    else{
        return -1;
    }
}



/*****************************test debug***********************/
u8 addr=7;
/*
test spuersonic sensor mesure distance
*/
void xp_supers_test(void){
    int val=0;

    // val=xp_supers_get_dis(6,supers_B,1);
    // println("M30 get distance=%d cm",val);
}


/*
cli debug
*/
int xp_supers_debug(char *type,char *fun,char *param){
    if(strcmp(type,"supers")!=0)return 0;
    if(strcmp(fun,"read")==0){
        int a=atoi(param);
        if(a>0&&a<8){
            addr=a;
            println("supersonic sensor %d address distance=%d",a,xp_supers_get_dis(a,supers_B,1));
        }
        else
        {
            println("error:supersonic sensor read address set=%d",a);
        }
    }


    else{
        return 0;
    }
    return 1;
}

//end of the file
